/* roboboca.h -- header principal da biblioteca roboboca
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogofsr@grad.icmc.usp.br>
 *                     Bruno Ribeiro de Oliveira
 *                     Arineiza Cristina Pinheiro
 *                     Tomas Grover <tomas.grover@usp.br>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

/** @file
 *   
 * Header principal da biblioteca roboboca.
 */

#ifndef ROBOBOCA_H_
#define ROBOBOCA_H_

#include <libplayerc/playerc.h>

/* Defines */
/** Número total de blobs passíveis de serem encontrados  */
#define BLOB_COUNT 5

/* Typedefs */
typedef struct roboboca_t roboboca_t;
typedef struct roboboca_robot_t roboboca_robot_t;
typedef struct blob_pos blob_pos;

/* Enums */
/** Marca o estado de um blob  */
enum blob_estado
{
    BLOB_VAZIO = -1,	       /**< Campo da estrutura vazio ainda  */
    BLOB_PEGO,		       /**< O Blob que tinha sido encontrado
				 * agora já foi pego  */
    BLOB_BUSCANDO,		/**< Esta sendo buscado, mas pode não
				 * ser um blob de verdade  */
    BLOB_ESPERANDO	       /**< Blob sentado esperando alguém ir
				 * dar atenção a ele  */
};

/* Structs */
/** Informações sobre um blob
 *
 * Marca a posição de um blob e qual o estado dele
 * */
struct blob_pos
{    
    enum blob_estado ta_la;	/**< Marca o estado do blob */
    double x;			/**< Posição X global */
    double y;			/**< Posição Y global */
};

/** Estutura pricipal de guarda roboboca_robot_t s  */
struct roboboca_t
{
    /** Um mutex para acessar o vetor de blobs  */
    pthread_mutex_t * mutex;
    /** Número de robôs que a estrutura pode guardar.  */
    int n_robots;
    /** Lista de robôs  */    
    roboboca_robot_t ** l_robots;
    /** Guarda struct blob_pos */
    blob_pos vec_blob_pos[BLOB_COUNT];
    int all_blobs_finded;
    int all_blobs_returned;
};

/** Um robô.
 *
 * Consiste em um uma estrutura cheio de ponteiros mas é,
 * conceitualmente, a representação de um robô
 *
 */
struct roboboca_robot_t
{
    playerc_client_t *client;	/**< Client da biblioteca player */
    playerc_laser_t *laser;	/**< Laser */
    playerc_position2d_t *position2d; /**< Odômetro */
    playerc_blobfinder_t *blobfinder; /**< Blobfinder */
    playerc_gripper_t *gripper;	      /**< Garra para pegar blobs */
    double x_orig;		      /**< Posição X global de início */
    double y_orig;		      /**< Posição Y global de início */
    blob_pos *vec_blob_pos;	      /**< Ponteiro para a estrutura que guarda as informações dos blobs */
};

/* roboboca_t *****************************************************************/
roboboca_t * roboboca_new( int n_robos );
int roboboca_destroy( struct roboboca_t *robos );
int roboboca_append_robot( roboboca_t * box, roboboca_robot_t *robot );
void roboboca_mark_false_blob( roboboca_t * box, const int blob_i );

/* roboboca_robot_t ***********************************************************/
roboboca_robot_t * roboboca_robot_new( const double x_orig, const double y_orig, const char *server, const int port );
int roboboca_robot_destroy( roboboca_robot_t * robot );
double roboboca_robot_get_global_px( roboboca_robot_t *robot );
double roboboca_robot_get_global_py( roboboca_robot_t *robot );
int roboboca_robot_grab_blob( roboboca_t * box, roboboca_robot_t * robot, int blob_i );
void roboboca_robot_point_to_blob( roboboca_robot_t * robot, const double blob_x, const double blob_y );
int roboboca_robot_focus_on_nearest_blob( roboboca_robot_t * robot );
int roboboca_robot_approach_blob( roboboca_robot_t * robot );
void roboboca_robot_return_blob( roboboca_t * box, roboboca_robot_t * robot, const int blob_i );

/* Funções para os Campos Potenciais */
int roboboca_SeekGoal( roboboca_robot_t *robot , double xgoal, double ygoal, double * xdelta, double * ydelta );
int roboboca_AvoidObstacle2( const double distance, double angle, double *xdelta, double *ydelta);
int roboboca_AvoidObstacle( const double distance, const double angle, double * xdelta, double * ydelta );
double roboboca_laser_to_global_rad( const double pa, const double laser );

double roboboca_minor_distance_rad( const double goal, const double pose );
void roboboca_get_blob_position( const unsigned int x_finder, const float r_finder, const double px, const double py, const double pa, double * x_blob, double * y_blob );
int roboboca_mark_blob( roboboca_t *box, const double x_blob, const double y_blob );

int roboboca_goto_simple( roboboca_t * box, roboboca_robot_t *robot, double px, double py, int searchingx );
int roboboca_get_nearest_blob( roboboca_t * box, roboboca_robot_t * robot );

#endif /*ROBOBOCA_H_*/
